home *** CD-ROM | disk | FTP | other *** search
/ FishMarket 1.0 / FishMarket v1.0.iso / fishies / 351-375 / disk_351 / pdc / libsrc.lzh / LibSrc / Math / cordic.c < prev    next >
C/C++ Source or Header  |  1990-04-07  |  17KB  |  361 lines
  1.  
  2. #include <stdio.h>
  3. #include <math.h>
  4.  
  5. #include "cordic.h"
  6.  
  7. extern double   frexp(), ldexp(), modf();
  8.  
  9. char            Table0_index[80] = {
  10.     0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15,
  11.     16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31,
  12.     32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47,
  13.     48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63,
  14.     64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79
  15. };
  16.  
  17. double          Table0_coeff[80] = {
  18.  
  19.     /* alpha[i]                 delta[i]         */
  20.     /* --------------------     ------------------   */
  21.  
  22.     7.8539816339744830961566084581988E-01,  /* ATAN( 2 ** -0    )    */
  23.     4.6364760900080611621425623146122E-01,  /* ATAN( 2 ** -1    )    */
  24.     2.4497866312686415417208248121128E-01,  /* ATAN( 2 ** -2    )    */
  25.     1.2435499454676143503135484916387E-01,  /* ATAN( 2 ** -3    )    */
  26.     6.2418809995957348473979112985505E-02,  /* ATAN( 2 ** -4    )    */
  27.     3.1239833430268276253711744892491E-02,  /* ATAN( 2 ** -5    )    */
  28.     1.5623728620476830802801521256570E-02,  /* ATAN( 2 ** -6    )    */
  29.     7.8123410601011112964633918421993E-03,  /* ATAN( 2 ** -7    )    */
  30.     3.9062301319669718276286653114244E-03,  /* ATAN( 2 ** -8    )    */
  31.     1.9531225164788186851214826250767E-03,  /* ATAN( 2 ** -9    )    */
  32.     9.7656218955931943040343019971729E-04,  /* ATAN( 2 ** -10   )    */
  33.     4.8828121119489827546923962564485E-04,  /* ATAN( 2 ** -11   )    */
  34.     2.4414062014936176401672294325966E-04,  /* ATAN( 2 ** -12   )    */
  35.     1.2207031189367020423905864611796E-04,  /* ATAN( 2 ** -13   )    */
  36.     6.1035156174208775021662569173829E-05,  /* ATAN( 2 ** -14   )    */
  37.     3.0517578115526096861825953438536E-05,  /* ATAN( 2 ** -15   )    */
  38.     1.5258789061315762107231935812698E-05,  /* ATAN( 2 ** -16   )    */
  39.     7.6293945311019702633884823401051E-06,  /* ATAN( 2 ** -17   )    */
  40.     3.8146972656064962829230756163730E-06,  /* ATAN( 2 ** -18   )    */
  41.     1.9073486328101870353653693059172E-06,  /* ATAN( 2 ** -19   )    */
  42.     9.5367431640596087942067068992311E-07,  /* ATAN( 2 ** -20   )    */
  43.     4.7683715820308885992758382144925E-07,  /* ATAN( 2 ** -21   )    */
  44.     2.3841857910155798249094797721893E-07,  /* ATAN( 2 ** -22   )    */
  45.     1.1920928955078068531136849713792E-07,  /* ATAN( 2 ** -23   )    */
  46.     5.9604644775390554413921062141789E-08,  /* ATAN( 2 ** -24   )    */
  47.     2.9802322387695303676740132767710E-08,  /* ATAN( 2 ** -25   )    */
  48.     1.4901161193847655147092516595963E-08,  /* ATAN( 2 ** -26   )    */
  49.     7.4505805969238279871365645744954E-09,  /* ATAN( 2 ** -27   )    */
  50.     3.7252902984619140452670705718119E-09,  /* ATAN( 2 ** -28   )    */
  51.     1.8626451492309570290958838214765E-09,  /* ATAN( 2 ** -29   )    */
  52.     9.3132257461547851535573547768456E-10,  /* ATAN( 2 ** -30   )    */
  53.     4.6566128730773925777884193471057E-10,  /* ATAN( 2 ** -31   )    */
  54.     2.3283064365386962890204274183882E-10,  /* ATAN( 2 ** -32   )    */
  55.     1.1641532182693481445259909272985E-10,  /* ATAN( 2 ** -33   )    */
  56.     5.8207660913467407226496761591232E-11,  /* ATAN( 2 ** -34   )    */
  57.     2.9103830456733703613273032698904E-11,  /* ATAN( 2 ** -35   )    */
  58.     1.4551915228366851806639597837363E-11,  /* ATAN( 2 ** -36   )    */
  59.     7.2759576141834259033201841046704E-12,  /* ATAN( 2 ** -37   )    */
  60.     3.6379788070917129516601402005838E-12,  /* ATAN( 2 ** -38   )    */
  61.     1.8189894035458564758300761188230E-12,  /* ATAN( 2 ** -39   )    */
  62.     9.0949470177292823791503881172787E-13,  /* ATAN( 2 ** -40   )    */
  63.     4.5474735088646411895751949990348E-13,  /* ATAN( 2 ** -41   )    */
  64.     2.2737367544323205947875976170669E-13,  /* ATAN( 2 ** -42   )    */
  65.     1.1368683772161602973937988232271E-13,  /* ATAN( 2 ** -43   )    */
  66.     5.6843418860808014869689941345026E-14,  /* ATAN( 2 ** -44   )    */
  67.     2.8421709430404007434844970695472E-14,  /* ATAN( 2 ** -45   )    */
  68.     1.4210854715202003717422485350606E-14,  /* ATAN( 2 ** -46   )    */
  69.     7.1054273576010018587112426756617E-15,  /* ATAN( 2 ** -47   )    */
  70.     3.5527136788005009293556213378757E-15,  /* ATAN( 2 ** -48   )    */
  71.     1.7763568394002504646778106689434E-15,  /* ATAN( 2 ** -49   )    */
  72.     8.8817841970012523233890533447242E-16,  /* ATAN( 2 ** -50   )    */
  73.     4.4408920985006261616945266723630E-16,  /* ATAN( 2 ** -51   )    */
  74.     2.2204460492503130808472633361816E-16,  /* ATAN( 2 ** -52   )    */
  75.     1.1102230246251565404236316680908E-16,  /* ATAN( 2 ** -53   )    */
  76.     5.5511151231257827021181583404541E-17,  /* ATAN( 2 ** -54   )    */
  77.     2.7755575615628913510590791702271E-17,  /* ATAN( 2 ** -55   )    */
  78.     1.3877787807814456755295395851135E-17,  /* ATAN( 2 ** -56   )    */
  79.     6.9388939039072283776476979255676E-18,  /* ATAN( 2 ** -57   )    */
  80.     3.4694469519536141888238489627838E-18,  /* ATAN( 2 ** -58   )    */
  81.     1.7347234759768070944119244813919E-18,  /* ATAN( 2 ** -59   )    */
  82.     8.6736173798840354720596224069595E-19,  /* ATAN( 2 ** -60   )    */
  83.     4.3368086899420177360298112034798E-19,  /* ATAN( 2 ** -61   )    */
  84.     2.1684043449710088680149056017399E-19,  /* ATAN( 2 ** -62   )    */
  85.     1.0842021724855044340074528008699E-19,  /* ATAN( 2 ** -63   )    */
  86.     5.4210108624275221700372640043497E-20,  /* ATAN( 2 ** -64   )    */
  87.     2.7105054312137610850186320021749E-20,  /* ATAN( 2 ** -65   )    */
  88.     1.3552527156068805425093160010874E-20,  /* ATAN( 2 ** -66   )    */
  89.     6.7762635780344027125465800054371E-21,  /* ATAN( 2 ** -67   )    */
  90.     3.3881317890172013562732900027186E-21,  /* ATAN( 2 ** -68   )    */
  91.     1.6940658945086006781366450013593E-21,  /* ATAN( 2 ** -69   )    */
  92.     8.4703294725430033906832250067964E-22,  /* ATAN( 2 ** -70   )    */
  93.     4.2351647362715016953416125033982E-22,  /* ATAN( 2 ** -71   )    */
  94.     2.1175823681357508476708062516991E-22,  /* ATAN( 2 ** -72   )    */
  95.     1.0587911840678754238354031258496E-22,  /* ATAN( 2 ** -73   )    */
  96.     5.2939559203393771191770156292478E-23,  /* ATAN( 2 ** -74   )    */
  97.     2.6469779601696885595885078146239E-23,  /* ATAN( 2 ** -75   )    */
  98.     1.3234889800848442797942539073119E-23,  /* ATAN( 2 ** -76   )    */
  99.     6.6174449004242213989712695365597E-24,  /* ATAN( 2 ** -77   )    */
  100.     3.3087224502121106994856347682799E-24,  /* ATAN( 2 ** -78   )    */
  101.     1.6543612251060553497428173841399E-24   /* ATAN( 2 ** -79   )    */
  102. };
  103.  
  104. CORDIC_Table    CORDIC_Table0[1] = {
  105.     60,                                     /* Table Size    */
  106.     1,                                      /* m = 1         */
  107.     1.6467602581210656483660512222823E+00,  /* value         */
  108.     Table0_index,                           /* indexes       */
  109.     Table0_coeff                            /* Coefficients  */
  110. };
  111.  
  112. char            Table1_index[80] = {
  113.     1, 2, 3, 4, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 13, 14,
  114.     15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30,
  115.     31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 40, 41, 42, 43, 44, 45,
  116.     46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61,
  117.     62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77
  118. };
  119.  
  120. double          Table1_coeff[80] = {
  121.     0.0000000000000000000000000000000E+00,  /* EMPTY             */
  122.     5.4930614433405484569762261846126E-01,  /* ATANH( 2 ** -1   )    */
  123.     2.5541281188299534160275704815183E-01,  /* ATANH( 2 ** -2   )    */
  124.     1.2565721414045303884256886520094E-01,  /* ATANH( 2 ** -3   )    */
  125.     6.2581571477003007126765023862207E-02,  /* ATANH( 2 ** -4   )    */
  126.     3.1260178490666994764012245172648E-02,  /* ATANH( 2 ** -5   )    */
  127.     1.5626271752052211379201778751638E-02,  /* ATANH( 2 ** -6   )    */
  128.     7.8126589515404209103234712760400E-03,  /* ATANH( 2 ** -7   )    */
  129.     3.9062698683968260531275633697076E-03,  /* ATANH( 2 ** -8   )    */
  130.     1.9531274835325499986507708868542E-03,  /* ATANH( 2 ** -9   )    */
  131.     9.7656281044103584096445002988456E-04,  /* ATANH( 2 ** -10  )    */
  132.     4.8828128880511282676100662627039E-04,  /* ATANH( 2 ** -11  )    */
  133.     2.4414062985063858292797225210244E-04,  /* ATANH( 2 ** -12  )    */
  134.     1.2207031310632980660296307873708E-04,  /* ATANH( 2 ** -13  )    */
  135.     6.1035156325791225317150609727698E-05,  /* ATANH( 2 ** -14  )    */
  136.     3.0517578134473903148761958402142E-05,  /* ATANH( 2 ** -15  )    */
  137.     1.5258789063684237893098936432131E-05,  /* ATANH( 2 ** -16  )    */
  138.     7.6293945313980297366218574175503E-06,  /* ATANH( 2 ** -17  )    */
  139.     3.8146972656435037170772475010538E-06,  /* ATANH( 2 ** -18  )    */
  140.     1.9073486328148129646346407914542E-06,  /* ATANH( 2 ** -19  )    */
  141.     9.5367431640653912057932962562050E-07,  /* ATANH( 2 ** -20  )    */
  142.     4.7683715820316114007241618841150E-07,  /* ATANH( 2 ** -21  )    */
  143.     2.3841857910156701750905202231884E-07,  /* ATANH( 2 ** -22  )    */
  144.     1.1920928955078181468863150284763E-07,  /* ATANH( 2 ** -23  )    */
  145.     5.9604644775390695586078937857760E-08,  /* ATANH( 2 ** -24  )    */
  146.     2.9802322387695321323259867232276E-08,  /* ATANH( 2 ** -25  )    */
  147.     1.4901161193847657352907483404036E-08,  /* ATANH( 2 ** -26  )    */
  148.     7.4505805969238282628634354255046E-09,  /* ATANH( 2 ** -27  )    */
  149.     3.7252902984619140797329292355951E-09,  /* ATANH( 2 ** -28  )    */
  150.     1.8626451492309570334041161664864E-09,  /* ATANH( 2 ** -29  )    */
  151.     9.3132257461547851589426452156312E-10,  /* ATANH( 2 ** -30  )    */
  152.     4.6566128730773925784615806524241E-10,  /* ATANH( 2 ** -31  )    */
  153.     2.3283064365386962891045725815824E-10,  /* ATANH( 2 ** -32  )    */
  154.     1.1641532182693481445365090726996E-10,  /* ATANH( 2 ** -33  )    */
  155.     5.8207660913467407226628238408757E-11,  /* ATANH( 2 ** -34  )    */
  156.     2.9103830456733703613289467301095E-11,  /* ATANH( 2 ** -35  )    */
  157.     1.4551915228366851806641652162637E-11,  /* ATANH( 2 ** -36  )    */
  158.     7.2759576141834259033200557093408E-12,  /* ATANH( 2 ** -37  )    */
  159.     3.6379788070917129516601241511676E-12,  /* ATANH( 2 ** -38  )    */
  160.     1.8189894035458564758300741126460E-12,  /* ATANH( 2 ** -39  )    */
  161.     9.0949470177292823791503856095574E-13,  /* ATANH( 2 ** -40  )    */
  162.     4.5474735088646411895751946855697E-13,  /* ATANH( 2 ** -41  )    */
  163.     2.2737367544323205947875975778837E-13,  /* ATANH( 2 ** -42  )    */
  164.     1.1368683772161602973937988183292E-13,  /* ATANH( 2 ** -43  )    */
  165.     5.6843418860808014869689941283803E-14,  /* ATANH( 2 ** -44  )    */
  166.     2.8421709430404007434844970687819E-14,  /* ATANH( 2 ** -45  )    */
  167.     1.4210854715202003717422485349649E-14,  /* ATANH( 2 ** -46  )    */
  168.     7.1054273576010018587112426755421E-15,  /* ATANH( 2 ** -47  )    */
  169.     3.5527136788005009293556213378607E-15,  /* ATANH( 2 ** -48  )    */
  170.     1.7763568394002504646778106689416E-15,  /* ATANH( 2 ** -49  )    */
  171.     8.8817841970012523233890533447219E-16,  /* ATANH( 2 ** -50  )    */
  172.     4.4408920985006261616945266723627E-16,  /* ATANH( 2 ** -51  )    */
  173.     2.2204460492503130808472633361816E-16,  /* ATANH( 2 ** -52  )    */
  174.     1.1102230246251565404236316680908E-16,  /* ATANH( 2 ** -53  )    */
  175.     5.5511151231257827021181583404541E-17,  /* ATANH( 2 ** -54  )    */
  176.     2.7755575615628912740218813947376E-17,  /* ATANH( 2 ** -55  )    */
  177.     1.3877787807814456562702401412412E-17,  /* ATANH( 2 ** -56  )    */
  178.     6.9388939039072283294994493158867E-18,  /* ATANH( 2 ** -57  )    */
  179.     3.4694469519536141767867868103636E-18,  /* ATANH( 2 ** -58  )    */
  180.     1.7347234759768070914026589432869E-18,  /* ATANH( 2 ** -59  )    */
  181.     8.6736173798840354645364585616969E-19,  /* ATANH( 2 ** -60  )    */
  182.     4.3368086899420177341490202421641E-19,  /* ATANH( 2 ** -61  )    */
  183.     2.1684043449710088675447078614110E-19,  /* ATANH( 2 ** -62  )    */
  184.     1.0842021724855044338899033657877E-19,  /* ATANH( 2 ** -63  )    */
  185.     5.4210108624275221697433904166441E-20,  /* ATANH( 2 ** -64  )    */
  186.     2.7105054312137610849451636052485E-20,  /* ATANH( 2 ** -65  )    */
  187.     1.3552527156068805424909489018558E-20,  /* ATANH( 2 ** -66  )    */
  188.     6.7762635780344027125006622573581E-21,  /* ATANH( 2 ** -67  )    */
  189.     3.3881317890172013562618105656988E-21,  /* ATANH( 2 ** -68  )    */
  190.     1.6940658945086006781337751421043E-21,  /* ATANH( 2 ** -69  )    */
  191.     8.4703294725430033906760503586591E-22,  /* ATANH( 2 ** -70  )    */
  192.     4.2351647362715016953398188413639E-22,  /* ATANH( 2 ** -71  )    */
  193.     2.1175823681357508476703578361905E-22,  /* ATANH( 2 ** -72  )    */
  194.     1.0587911840678754238352910219724E-22,  /* ATANH( 2 ** -73  )    */
  195.     5.2939559203393771191767353695549E-23,  /* ATANH( 2 ** -74  )    */
  196.     2.6469779601696885595884377497007E-23,  /* ATANH( 2 ** -75  )    */
  197.     1.3234889800848442797942363910811E-23,  /* ATANH( 2 ** -76  )    */
  198.     6.6174449004242213989712257459827E-24,  /* ATANH( 2 ** -77  )    */
  199.     3.3087224502121106994856238206356E-24,  /* ATANH( 2 ** -78  )    */
  200.     1.6543612251060553497428146472289E-24,  /* ATANH( 2 ** -79  )    */
  201. };
  202.  
  203. CORDIC_Table    CORDIC_Table1[1] = {
  204.     60,                           /* Table Size    */
  205.     -1,                           /* m = -1        */
  206.     8.28159360960215359E-01,      /* K value       */
  207.     Table1_index,                 /* indexes       */
  208.     Table1_coeff                  /* Coefficients  */
  209. };
  210.  
  211. void
  212. CORDIC_rotate0(method, tbl, x1, y1, z1)
  213.     int             method;
  214.     CORDIC_Table   *tbl;
  215.     double         *x1, *y1, *z1;
  216. {
  217.     int             i, idx;
  218.     double          xn, yn, zn;
  219.     double          x0, y0, z0;
  220.     int             ix, iy, iw;
  221.  
  222.     x0 = *x1;
  223.     y0 = *y1;
  224.     z0 = *z1;
  225.  
  226.     if (method == 0) {
  227.         for (i = 0; i < tbl->size; i++) {
  228.             idx = tbl->index[i];
  229.             xn = frexp(x0, &ix);
  230.             xn = ldexp(xn, ix - idx);
  231.             yn = frexp(y0, &iy);
  232.             yn = ldexp(yn, iy - idx);
  233.             if ((x0 >= 0.0 && y0 >= 0.0) || (x0 <= 0.0 && y0 <= 0.0)) {
  234.                 x0 += yn;
  235.                 y0 -= xn;
  236.                 z0 += tbl->coeff[idx];
  237.             }
  238.             else {
  239.                 x0 -= yn;
  240.                 y0 += xn;
  241.                 z0 -= tbl->coeff[idx];
  242.             }
  243.         }
  244.     }
  245.     else {
  246.         for (i = 0; i < tbl->size; i++) {
  247.             idx = tbl->index[i];
  248.             xn = frexp(x0, &ix);
  249.             xn = ldexp(xn, ix - idx);
  250.             yn = frexp(y0, &iy);
  251.             yn = ldexp(yn, iy - idx);
  252.             if (z0 < 0.0) {
  253.                 x0 += yn;
  254.                 y0 -= xn;
  255.                 z0 += tbl->coeff[idx];
  256.             }
  257.             else {
  258.                 x0 -= yn;
  259.                 y0 += xn;
  260.                 z0 -= tbl->coeff[idx];
  261.             }
  262.         }
  263.     }
  264.     *x1 = x0;
  265.     *y1 = y0;
  266.     *z1 = z0;
  267. }
  268.  
  269. void
  270. CORDIC_rotate1(method, tbl, x1, y1, z1)
  271.     int             method;
  272.     CORDIC_Table   *tbl;
  273.     double         *x1, *y1, *z1;
  274. {
  275.     int             i, idx;
  276.     double          xn, yn, zn;
  277.     double          x0, y0, z0;
  278.     int             ix, iy, iw;
  279.  
  280.     x0 = *x1;
  281.     y0 = *y1;
  282.     z0 = *z1;
  283.  
  284.     if (method == 0) {
  285.         for (i = 0; i < tbl->size; i++) {
  286.             idx = tbl->index[i];
  287.             xn = frexp(x0, &ix);
  288.             xn = ldexp(xn, ix - idx);
  289.             yn = frexp(y0, &iy);
  290.             yn = ldexp(yn, iy - idx);
  291.             if ((x0 >= 0.0 && y0 >= 0.0) || (x0 <= 0.0 && y0 <= 0.0)) {
  292.                 x0 -= yn;
  293.                 y0 -= xn;
  294.                 z0 += tbl->coeff[idx];
  295.             }
  296.             else {
  297.                 x0 += yn;
  298.                 y0 += xn;
  299.                 z0 -= tbl->coeff[idx];
  300.             }
  301.         }
  302.     }
  303.     else {
  304.         for (i = 0; i < tbl->size; i++) {
  305.             idx = tbl->index[i];
  306.             xn = frexp(x0, &ix);
  307.             xn = ldexp(xn, ix - idx);
  308.             yn = frexp(y0, &iy);
  309.             yn = ldexp(yn, iy - idx);
  310.             if (z0 < 0.0) {
  311.                 x0 -= yn;
  312.                 y0 -= xn;
  313.                 z0 += tbl->coeff[idx];
  314.             }
  315.             else {
  316.                 x0 += yn;
  317.                 y0 += xn;
  318.                 z0 -= tbl->coeff[idx];
  319.             }
  320.         }
  321.     }
  322.     *x1 = x0;
  323.     *y1 = y0;
  324.     *z1 = z0;
  325. }
  326.  
  327. #if 0
  328.  
  329. main()
  330. {
  331.     double          x, y, z;
  332.  
  333.     x = 1.0;
  334.     y = 1.0;
  335.     z = 0.0;
  336.  
  337.     CORDIC_rotate0(0, CORDIC_Table0, &x, &y, &z);
  338.     printf("Method = %d (x = %g) (y = %g) (z = %g)\n", 0, x, y, z);
  339.  
  340.     x = -1.0;
  341.     y = 1.0;
  342.     z = 0.0;
  343.     CORDIC_rotate0(1, CORDIC_Table0, &x, &y, &z);
  344.     printf("Method = %d (x = %g) (y = %g) (z = %g)\n", 1, x, y, z);
  345.  
  346.     x = 1.5;
  347.     y = -1.0;
  348.     z = 0.0;
  349.  
  350.     CORDIC_rotate1(0, CORDIC_Table1, &x, &y, &z);
  351.     printf("Method = %d (x = %g) (y = %g) (z = %g)\n", 0, x, y, z);
  352.  
  353.     x = -1.0;
  354.     y = -1.0;
  355.     z = 0.5;
  356.     CORDIC_rotate1(1, CORDIC_Table1, &x, &y, &z);
  357.     printf("Method = %d (x = %g) (y = %g) (z = %g)\n", 1, x, y, z);
  358. }
  359.  
  360. #endif
  361.